;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;	Stereo Disparity by a Cooperative Method -- 2-dimensional image      ;;;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;
;;; Input images binary, fixnum, 100*100 (0 or 1) and are held in LEFT and RIGHT
;;; Disparity bindings are held in the array ADISP
;;; and their initial values are held in INITIALDISP
;;;
;;; To load the network, call (LOADDISP).  To run it, call
;;; (RUNI N 'name), which runs the system through N iterations using information about
;;; the initial conditions on every iteration. Output is stored under filenames
;;; derived from <name>. 
;;;

(DECLARE (ARRAY* (FLONUM ADISP 3. NEWDISP 3.)
                 (FIXNUM LEFT 2. RIGHT 2. SHOW 2. VIEW 2.))
         (*LEXPR P Q))

(DECLARE (FIXNUM L R DISP)
         (FLONUM VALUE))

(DECLARE (SPECIAL SIGMOID-SLOPE SIGMOID-THRESHOLD)
         (FLONUM VALUE SIGMOID-SLOPE SIGMOID-THRESHOLD (SIGMOID FLONUM)))

(DEFUN SIGMOID (VALUE)
       (COND ((< VALUE SIGMOID-THRESHOLD) 0.0)
             (T (MIN 1.0 (*$ SIGMOID-SLOPE (-$ VALUE SIGMOID-THRESHOLD))))))

(DECLARE (SPECIAL EPSILON M)
         (FLONUM EPSILON VALUE)
         (FIXNUM XL YX XR M UL K DISP XD YD)
         (NOTYPE NBD ELS EL))

(SETQ M 5 EPSILON 2.0 SIGMOID-SLOPE 1.0 SIGMOID-THRESHOLD 3.0)

(DECLARE (FLONUM INITIALVALUE))

(DEFUN FASTNEWVALUEI (XL YL XR INITIALVALUE NBD)
       ;;; uses initial values -- have to extract them each time as
       ;;; there is no room in core for a copy of the initial array
       (PROG (VALUE DISP EL XD YD UL)
             (SETQ VALUE 0.0 DISP (- XR XL -3))
             ;;; pick up excitation
             (DO ELS NBD (CDR ELS) (NULL ELS)
                 (SETQ EL (CAR ELS)
                       XD (CAR EL)
                       YD (CADR EL))
                 (SETQ VALUE (+$ VALUE
                                 (ADISP (+ XL XD) (+ YL YD) DISP))))
             ;;; pick up inhibition parallel to RIGHT axis
	     (SETQ UL (+ XR 4))
             (DO K (- XR 3) (1+ K) (= K UL)
                 (SETQ VALUE (-$ VALUE (*$ EPSILON (ADISP K YL (- XR K -3))))))
             ;;; now parallel to LEFT axis
	     (SETQ UL (+ XL 4))
             (DO K (- XL 3) (1+ K) (= K UL)
                 (SETQ VALUE (-$ VALUE (*$ EPSILON (ADISP XL YL (- K XL -3))))))
             ;;; don't want to subtract itself twice
             ;;; also add in starting conditions
             (SETQ VALUE (+$ VALUE INITIALVALUE (*$ 2.0 EPSILON (ADISP XL YL DISP))))
             (STORE (NEWDISP XL YL DISP) (SIGMOID VALUE))
             (RETURN T)))

(DECLARE (FIXNUM I J LEFTEYE RIGHTEYE Y))

(DEFUN LOADDISP NIL
       ;;; loads the initial DISP array from data arrays LEFT and RIGHT
       (PROG (LEFTEYE RIGHTEYE)
	   (FILLARRAY 'ADISP '(0.0))
           (DO Y 3 (1+ Y) (= Y 97.)
             (DO I 3 (1+ I) (= I 97.)
		 (COND ((= (LEFT I Y) 0.) (GO CONTINUE))
                       (T (SETQ LEFTEYE I)))
                 (DO J -3 (1+ J) (= J 4.)
                     (SETQ RIGHTEYE (+ LEFTEYE J))
                     (COND ((= (RIGHT RIGHTEYE Y) 1.)
                            (STORE (ADISP LEFTEYE Y (+ J 3)) 1.0))))
                 CONTINUE))))

(DECLARE (SPECIAL DISPLAY-FLAG) 
         (FIXNUM M-BY-2)
         (NOTYPE DISPLAY-FLAG FN))

(SETQ DISPLAY-FLAG T)

(DECLARE (ARRAY* (NOTYPE CIRCLE 1.)))

(DECLARE (SPECIAL INPUT-CONTRIBUTION SHOWFLAG XGPFLAG PWFLAG FIRST-XGP LAST-XGP)
	 (FLONUM INPUT-CONTRIBUTION IVAL INITIALVALUE)
         (NOTYPE LEFTFLAG FN SHOWFLAG XGPFLAG)
         (NOTYPE (FASTNEWVALUEI FIXNUM FIXNUM FIXNUM FLONUM NOTYPE)))

(SETQ INPUT-CONTRIBUTION 1.0)

(DEFUN RUNI (N FN)
       ;;; uses initial data each iteration
       (PROG (UL M-BY-2 NBD INITIALVALUE IVAL LEFTFLAG AUTOTHRESH)
	     (SETQ FIRST-XGP T LAST-XGP NIL)
	     (P '(SHALL I SAVE SHOW EACH ITERATION?))
	     (SETQ SHOWFLAG (Q))
	     (P '(SHALL I CREATE AN XGP IMAGE FOR EACH ITERATION?))
	     (SETQ XGPFLAG (Q))
	     (SETQ DISPLAY-FLAG (OR SHOWFLAG XGPFLAG))
             (P '(SHALL I CREATE A PHOTOWRITER FILE FOR EACH ITERATION?))
             (SETQ PWFLAG (Q))
	     (SETQ IVAL INPUT-CONTRIBUTION)
             (SETQ M-BY-2 (// M 2))
             (SETQ NBD (CIRCLE M-BY-2))
             (AND PWFLAG (PW-DSPLY FN 0.))
             (AND DISPLAY-FLAG (DSPLY FN 0.) (P 'INITIAL 'STATE))
	     (SETQ FIRST-XGP NIL)
             (P 'SHALL 'I 'SET 'SIGMOID-THRESHOLD 'AUTOMATICALLY?)
             (COND ((SETQ AUTOTHRESH (Q))
                    (P 'PARAMETERS 'ARE 'M '= M 'EPSILON '= EPSILON
		       'INPUT-CONTRIBUTION '= IVAL)
                    (P 'SIGMOID-THRESHOLD '= 'AUTOMATIC
                       'SIGMOID-SLOPE '= SIGMOID-SLOPE))
                   (T (P 'PARAMETERS 'ARE 'M '= M 'EPSILON '= EPSILON
			 'INPUT-CONTRIBUTION '= IVAL)
	              (P 'SIGMOID-THRESHOLD '= SIGMOID-THRESHOLD
                	 'SIGMOID-SLOPE '= SIGMOID-SLOPE)))
             (DO K 0 (1+ K) (= K N)
		;;; if last iteration, say so
		(SETQ LAST-XGP (= K (1- N)))
                ;;; if on automatic threshold mode, set
                ;;; sigmoid-threshold at each iteration
                (COND (AUTOTHRESH (SET-SIGMOID-THRESHOLD)))
		(DO Y 4 (1+ Y) (= Y 96.)
                 (DO I 6 (1+ I) (= I 94.)
                     ;;; set flag if there is a 1 in the left image
                     (SETQ LEFTFLAG (= (LEFT I Y) 1.))
                     (SETQ UL (+ I 4))
                     ;;; for speed, write two copies of code to avoid repeated test
                     (COND (LEFTFLAG (DO J (- I 3) (1+ J) (= J UL)
                                         (COND ((= (RIGHT J Y) 1.)
                                                (SETQ INITIALVALUE IVAL))
                                               (T (SETQ INITIALVALUE 0.0)))
                                          (FASTNEWVALUEI I Y J INITIALVALUE NBD)))
                           (T (DO J (- I 3) (1+ J) (= J UL)
                                  (FASTNEWVALUEI I Y J 0.0 NBD))))))
                 (FILLARRAY 'ADISP 'NEWDISP)
                 (AND DISPLAY-FLAG (DSPLY FN (1+ K)))
                 (AND PWFLAG (PW-DSPLY FN (1+ K)))
                 (P 'ITERATION (1+ K) 'DONE '(using initial values)))))

(DECLARE (FLONUM VALUE)
         (FIXNUM I J K))

(DEFUN SET-SIGMOID-THRESHOLD NIL
       (PROG (VALUE)
             (SETQ VALUE 0.0)
             ;;; find average value in a sample of adisp
             (DO K 0 (1+ K) (= K 7.)
                 (DO J 15. (1+ J) (= J 55.)
                     (DO I 15. (1+ I) (= I 55.)
                         (SETQ VALUE (+$ VALUE (ADISP I J K))))))
             (SETQ VALUE (//$ VALUE 11200.0))
             ;;; value is the average activity in the network at this point
             (P 'AVERAGE 'ACTIVITY 'IS VALUE)
	     (COND ((< VALUE 0.07) (SETQ SIGMOID-THRESHOLD 1.0))
		   ((< VALUE 0.11) (SETQ SIGMOID-THRESHOLD 2.0))
		   (T (SETQ SIGMOID-THRESHOLD 3.0)))
	     (P 'SIGMOID-THRESHOLD 'SET 'AT SIGMOID-THRESHOLD)
             (RETURN T)))

(DECLARE (FLONUM VALUE DISPFL V WT)
         (SPECIAL WINDOW))

(SETQ WINDOW '((0 0) (99. 99.)))
(SETQ TTYSEE-FUNCTION 'SHOW)

(DEFUN DSPLY (FN ITERATION)
       (PROG (DISP VALUE V WT)
             ;;; normalisation
             (FILLARRAY 'SHOW '(30.))
             (DO XL 6 (1+ XL) (= XL 94.)
                 (DO YL 4 (1+ YL) (= YL 96.)
                     (SETQ DISP 0 VALUE 0.0 WT 0.0)
                     (DO DISPFL 0.0 (1+$ DISPFL) (= DISPFL 7.0)
                         (SETQ V (ADISP XL YL DISP)
                               WT (+$ WT V)
                               VALUE (+$ VALUE (*$ DISPFL V)))
                         (SETQ DISP (1+ DISP)))
                     (STORE (SHOW XL YL) (FIX (//$ (*$ VALUE 10.0) WT)))))
		(AND XGPFLAG (XGPSHOW ITERATION FN))
		(AND SHOWFLAG (DUMPARRAYS '(SHOW) (LIST FN '> 'DSK 'MARR)))
             (RETURN T)))


(DEFUN PW-DSPLY (FN ITERATION)
       (PROG (DISP VALUE V WT)
             ;;; normalisation
             (FILLARRAY 'VIEW '(120.))
             (DO XL 6 (1+ XL) (= XL 94.)
                 (DO YL 4 (1+ YL) (= YL 96.)
                     (SETQ DISP 0 VALUE 0.0 WT 0.0)
                     (DO DISPFL 0.0 (1+$ DISPFL) (= DISPFL 7.0)
                         (SETQ V (ADISP XL YL DISP)
                               WT (+$ WT V)
                               VALUE (+$ VALUE (*$ DISPFL V)))
                         (SETQ DISP (1+ DISP)))
                     (STORE (VIEW (+ 14. XL) (+ 14. YL))
                            (FIX (//$ (*$ VALUE 40.0) WT)))))
             (PICTURE 'VIEW (LIST (IMPLODE (CONS 'P (EXPLODE FN))) ITERATION 'AI 'MARR))
             (RETURN T)))                     
                                         
βββ